#ifndef _VEHICLE_MONITOR_H_
#define _VEHICLE_MONITOR_H_

#include <dispatch_server/revise_vehicle_path.h>


namespace moying
{
namespace navigation
{


ReviseVehiclePath::ReviseVehiclePath(const VehiclePtr &vehicle,MoyingMapPtr moying_map){

    last_check_time_ = std::chrono::system_clock::now();
    vehicle_ = vehicle;
    moying_map_ = moying_map;
}

bool ReviseVehiclePath::checkWhetherNeedRevisePath(int cur_path_point_index){

    // WARNING("enter checkWhetherNeedRevisePath,path_index:"<<cur_path_point_index);
    if(vehicle_ == nullptr || moying_map_ == nullptr || cur_path_point_index < 0)
        return false;

    //是否刚刚检测过路径修正
    if( last_check_time_ + std::chrono::seconds(revise_check_interval_) > std::chrono::system_clock::now() )
        return false;
    //是否已经发送了路径规划请求
    if(vehicle_->recv_path_plan_req()){
        // WARNING("already send revise request");
        return false;
    }

    //死锁处理中的机器人不处理
    if(vehicle_->deadlock_group_id() > 0){
        // WARNING("robot in deadlock group");

        return false;
    }

    /****判断是否可以尝试规划更优的路径****/
    //检测是否在同一个位置等待太久
    int wait_in_place_seconds = 3;
    float time_res = moying_map_->path_plan_param().time_map_resolution;
    float map_plan_res = moying_map_->path_plan_param().map_plan_resolution;
    if(time_res == 0 || map_plan_res == 0)
        return false;
    const auto& node_path = vehicle_->path_plan_result().node_path;

    int cell_count = wait_in_place_seconds / time_res;
    if(cur_path_point_index + cell_count >= node_path.size())
        return false;
    Node c1,c2,c3;
    c1 = node_path.at(cur_path_point_index);
    c2 = node_path.at(cur_path_point_index + cell_count);
    if( c1.isEqual(c2) ){
        WARNING("wait too long ,revise path,robot_id:"<<vehicle_->getID());
        last_check_time_ = std::chrono::system_clock::now();
        return true;
    }

    //判断是否走弯路
    int check_length = 8;//检测的路径长度,单位米
    cell_count = check_length / map_plan_res;
    if(cur_path_point_index + cell_count >= node_path.size()){
        cell_count = node_path.size() - cur_path_point_index-2;
        if(cell_count < 3)
            return false;
    }
    int corner_count = 0;
    //计算拐点数
    for(int i=cur_path_point_index;i<cur_path_point_index + cell_count-1;i++){
        c1 = node_path.at(i);
        c2 = node_path.at(i + 1);
        c3 = node_path.at(i + 2);
        if( (c1.x == c2.x && c1.x == c3.x) || (c1.y == c2.y && c1.y == c3.y) ){
            continue;
        }
        corner_count++;
    }
    if(corner_count > 3){  //如果拐点数大于特定值就尝试重规划路径
        WARNING("corner is too much ,revise path,robot_id:"<<vehicle_->getID()<<" corner_count:"<<corner_count);
        last_check_time_ = std::chrono::system_clock::now();
        return true;
    }else{
        return false;
    }
    /*******/
}



}
}
#endif //_VEHICLE_MONITOR_H_